#include "testplugin.h"
#include "Poco/ClassLibrary.h"
#include "aqnode.h"
#include <iostream>

//namespace aq{



    /*===========================================================
     * Summary:
     * Performs gaussian convolution of any size sigma very fast by using
     * both image pyramids and seperable filters.  Recursion is used.
     *
     * Arguments:
     * img - an IplImage to be filtered in place.
     * double sigma - the standard deviation of the gaussian kernel to use.
    =============================================================
     */

    void MsrcrNode::FastFilter(cv::Mat& src, double sigma){


        int filter_size;

        // Reject unreasonable demands

        if( sigma > 300){
            sigma = 300;
        }

        // get needed filter size (enforce oddness)

        filter_size = static_cast<int>(floor(sigma * 6) / 2);
        filter_size = filter_size * 2 + 1;

        // if 3 sigma is less than a pixel, why bother (ie sigma < 2/3)

        if(filter_size < 3){
            return ;
        }

        // Filter, or downsample and recurse

        if(filter_size < 10){
            cv::GaussianBlur(src, src, cv::Size(filter_size, filter_size), 0, 0);
        }
        else{

            if(src.cols < 2 || src.rows < 2) return;
            cv::Mat sub_src = cv::Mat::zeros(cv::Size(src.cols / 2, src.rows / 2), CV_32FC3);
            cv::pyrDown( src, sub_src);
            FastFilter(sub_src, sigma / 2.0);
            cv::resize( sub_src, src, cv::Size(src.cols, src.rows));

        }

    }


    /*===========================================================

     * Summary:
     * Multiscale retinex restoration with color restoration.  The image and a set of
     * filtered images are converted to the log domain and subtracted from the
     * original with some set of weights. Typicaly called with three equaly weighted
     * scales of fine, medium and wide standard deviations. A color restoration weight
     * is then applied to each color channel.
     *
     * Arguments:
     * Mat src - Input Image.
     * Mat &dst - Output Image.
     * double sigma - the standard deviation of the gaussian kernal used to filter.
     * int gain - the factor by which to scale the image back into visable range.
     * int offset - an offset similar to the gain.
     * double restoration_factor - controls the non-linearaty of the color restoration.
     * double color_gain - controls the color restoration gain.
    =============================================================
     */

    void MsrcrNode::MultiScaleRetinexCR(const cv::Mat &src, cv::Mat &dst, std::vector<int> sigmas,
                                        int gain, int offset, double restoration_factor, double color_gain){

        std::vector<double> msrcr_weight;
        for(int i = 0; i < 3; i++){
            msrcr_weight.push_back(1. / 3);
        }

        cv::Mat src_copy;
        src_copy = src.clone();

        double weight = 0.;
        int scales = sigmas.size();

        cv::Mat A, fA, fB, fC, fsA, fsB, fsC, fsD;


        // Compute log image


        src_copy.convertTo(fB, CV_32FC3);
        cv::log(fB, fA);

        // Normalize according to given weights

        for (int i = 0; i < scales; i++ ){
            weight += msrcr_weight[i];
        }
        if( weight != 1.0) fA.convertTo(fA, CV_32FC3, weight);

        // Filter at each scale

        for(int i =0; i < scales; i++){

            A = src_copy.clone();
            FastFilter( A, sigmas[i]);
            A.convertTo( fB, CV_32FC3);
            cv::log( fB, fC);

            // Compute weighted difference


            fC.convertTo( fC, CV_32FC3, msrcr_weight[i]);
            cv::subtract( fA, fC, fA );

        }


        // Color Restore

        if(src_copy.channels() > 1){


            // Divide image into channels, convert and store sum

            cv::Mat src_copy_rgbchannel[3];
            cv::split(src_copy, src_copy_rgbchannel);

            src_copy_rgbchannel[0].convertTo(fsA, CV_32FC3);
            src_copy_rgbchannel[1].convertTo(fsB, CV_32FC3);
            src_copy_rgbchannel[2].convertTo( fsC, CV_32FC3);

            // Sum components

            cv::add(fsA, fsB, fsD);
            cv::add(fsD, fsC, fsD);

            // Normalize weights

            cv::divide(fsA, fsD, fsA, restoration_factor);
            cv::divide(fsB, fsD, fsB, restoration_factor);
            cv::divide(fsC, fsD, fsC, restoration_factor);

            fsA.convertTo(fsA, CV_32FC1, 1, 1);
            fsB.convertTo(fsA, CV_32FC1, 1, 1);
            fsC.convertTo(fsA, CV_32FC1, 1, 1);

            // Log weights

            cv::log(fsA, fsA);
            cv::log(fsB, fsB);
            cv::log(fsC, fsC);

            // Divide retinex image, weight accordingly and recombine

            cv::Mat fs[3];
            cv::split( fA, fs);
            cv::multiply(fs[0], fsA, fs[0], color_gain);
            cv::multiply(fs[1], fsB, fs[1], color_gain);
            cv::multiply(fs[2], fsC, fs[2], color_gain);

            cv::merge(fs, 3, fA);

        }

        // Restore

        fA.convertTo( dst, CV_8UC3, gain, offset);

    }





//// optional set up and clean up functions
//void pocoInitializeLibrary()
//{
//std::cout << "PluginLibrary initializing" << std::endl;
//}
//void pocoUninitializeLibrary()
//{
//std::cout << "PluginLibrary uninitializing" << std::endl;
//}

//}

